#include "arm_task.h"
#include "main.h"
// #include "cmsis_os.h"
//#include "arm_math.h"
#include "MOTOR_receive.h"
#include "user_lib.h"
// #include "detect_task.h"
#include "remote_control.h"
#include "arm_behaviour.h"
// #include "arm_cup.h"
#include "pid.h"
/*---------------野火PID助手协议-------------*/
#include "pid-protocol.h"//
extern UART_HandleTypeDef huart1;
/*---------------野火PID助手协议-------------*/

#define arm_total_pid_clear(arm_clear)                                                   \
    {                                                                                          \
                                                                                               \
        arm_PID_clear(&(arm_clear)->arm_motor5_motor.arm_motor_relative_angle_pid); \
        PID_clear(&(arm_clear)->arm_motor5_motor.arm_motor_gyro_pid);                  \
                                                                                               \
        arm_PID_clear(&(arm_clear)->arm_motor6_motor.arm_motor_relative_angle_pid); \
        PID_clear(&(arm_clear)->arm_motor6_motor.arm_motor_gyro_pid);                  \
    }

/**
  * @brief          初始化"arm_control"变量，包括pid初始化， 遥控器指针初始化，机械臂电机指针初始化，陀螺仪角度指针初始化
  * @param[out]     init:"arm_control"变量指针.
  * @retval         none
  */
static void arm_init(arm_control_t *init);
static void arm_offset_init(arm_control_t *init);

/**
  * @brief          设置机械臂控制模式，主要在'arm_behaviour_mode_set'函数中改变
  * @param[out]     arm_set_mode:"arm_control"变量指针.
  * @retval         none
  */
static void arm_set_mode(arm_control_t *set_mode);

/**
  * @brief          机械臂测量数据更新，包括电机速度，欧拉角度
  * @param[out]     arm_feedback_update:"arm_control"变量指针.
  * @retval         none
  */
static void arm_feedback_update(arm_control_t *feedback_update);

/**
  * @brief          机械臂模式改变，有些参数需要改变，例如控制yaw角度设定值应该变成当前yaw角度
  * @param[out]     arm_mode_change:"arm_control"变量指针.
  * @retval         none
  */
static void arm_mode_change_control_transit(arm_control_t *arm_mode_change);

/**
  * @brief          设置机械臂控制设定值，控制值是通过arm_behaviour_control_set函数设置的
  * @param[out]     arm_set_control:"arm_control"变量指针.
  * @retval         none
  */
static void arm_set_control(arm_control_t *set_control);

/**
  * @brief          控制循环，根据控制设定值，计算电机电流值，进行控制
  * @param[out]     arm_control_loop:"arm_control"变量指针.
  * @retval         none
  */
static void arm_control_loop(arm_control_t *control_loop);

/**
  * @brief          机械臂控制模式:ARM_MOTOR_ENCONDE，使用编码相对角进行控制
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
static void RM2006_joint_relative_angle_control(arm_rm_motor_t *arm_motor);
static void DM4310_joint_relative_angle_control(DM4310_Motor_t *arm_motor);
static void M8010_joint_relative_angle_control(M8010_Motor_t *arm_motor);
/**
  * @brief          机械臂控制模式:ARM_MOTOR_RAW，电流值直接发送到CAN总线.
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
static void RM2006_joint_raw_angle_control(arm_rm_motor_t *arm_motor);
static void DM4310_joint_raw_angle_control(DM4310_Motor_t *arm_motor);
static void M8010_joint_raw_angle_control(M8010_Motor_t *arm_motor);

/**
  * @brief          在ARM_MOTOR_ENCONDE模式，限制角度设定,防止超过最大
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
static void RM2006_joint_relative_angle_limit(arm_rm_motor_t *arm_motor,fp32 joint_angle);
static void DM4310_joint_relative_angle_limit(DM4310_Motor_t *arm_motor,fp32 joint_angle);
static void M8010_joint_relative_angle_limit(M8010_Motor_t *arm_motor,fp32 joint_angle);
/**
  * @brief          机械臂角度PID初始化, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:机械臂PID指针
  * @param[in]      maxout: pid最大输出
  * @param[in]      intergral_limit: pid最大积分输出
  * @param[in]      kp: pid kp
  * @param[in]      ki: pid ki
  * @param[in]      kd: pid kd
  * @retval         none
  */
static void arm_PID_init(arm_PID_t *pid, fp32 maxout, fp32 intergral_limit, fp32 kp, fp32 ki, fp32 kd);

/**
  * @brief          机械臂PID清除，清除pid的out,iout
  * @param[out]     pid_clear:"arm_control"变量指针.
  * @retval         none
  */
static void arm_PID_clear(arm_PID_t *pid_clear);

/**
  * @brief          机械臂角度PID计算, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:机械臂PID指针
  * @param[in]      get: 角度反馈
  * @param[in]      set: 角度设定
  * @param[in]      error_delta: 角速度
  * @retval         pid 输出
  */
static fp32 arm_PID_calc(arm_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);

/*-----------------------野火串口调试助手移植----------------*/
void set_pid_paramter_cmd(float p, float i, float d);
void set_pid_actual_val_cmd(int actual_val);
/*-----------------------野火串口调试助手移植---------------*/
//arm control data
//机械臂控制所有相关数据
arm_control_t arm_control;

//motor current 
//发送的电机电流
static int16_t  motor5_can_set_current = 0, motor6_can_set_current = 0;
static fp32  joint1_can_set_current = 0, joint2_usart_set_current = 0, joint3_usart_set_current = 0, joint4_can_set_current = 0;
/**
  * @brief          机械臂任务，间隔 arm_CONTROL_TIME 1ms
  * @param[in]      pvParameters: 空
  * @retval         none
  */

// void arm_task(void const *pvParameters)
void arm_task(void)
{
    //空闲一段时间
     //vTaskDelay(ARM_TASK_INIT_TIME);
    //机械臂初始化
    arm_init(&arm_control); //写自定义遥控器的同学注意，遥控器的数据获取接口在这里面。
    arm_offset_init(&arm_control);//初始化机械臂偏移量与角度范围
    //吸盘初始化
    // arm_cup_init();
//waw
    while (1)
    {       
        arm_set_mode(&arm_control);                    //设置机械臂控制模式
        arm_mode_change_control_transit(&arm_control); //控制模式切换 控制数据过渡
        arm_feedback_update(&arm_control);             //机械臂数据反馈
        arm_set_control(&arm_control);                 //设置机械臂控制量,
        arm_control_loop(&arm_control);                //机械臂电机控制参数计算

        // arm_cup_can_set_current = arm_cup_control_loop();        //吸盘任务控制循环,还没定义

        //控制量赋值
        joint1_can_set_current = arm_control.arm_joint1_motor.given_current;
        joint2_usart_set_current = arm_control.arm_joint2_motor.given_current;
        joint3_usart_set_current = arm_control.arm_joint3_motor.given_current;
        joint4_can_set_current = arm_control.arm_joint4_motor.given_current;
        motor5_can_set_current = arm_control.arm_motor5_motor.given_current;
        motor6_can_set_current = arm_control.arm_motor6_motor.given_current;

        /*------------------野火多功能调试助手PID协议-------------------------*/
        //速度环
	    // set_computer_value(SEND_TARGET_CMD,CURVES_CH3,&(arm_control.arm_motor6_motor.motor_gyro_set),1);
	    // set_computer_value(SEND_FACT_CMD,CURVES_CH3,&(arm_control.arm_motor6_motor.motor_gyro),1);
        //角度环
        // set_computer_value(SEND_TARGET_CMD,CURVES_CH1,&(arm_control.arm_motor6_motor.relative_angle_set),1);
	    // set_computer_value(SEND_FACT_CMD,CURVES_CH1,&(arm_control.arm_motor6_motor.relative_angle),1);
        /*----------------------野火多功能调试助手PID协议-------------------------*/

        //发送控制量
        FDCAN_M2006_cmd_arm(motor6_can_set_current, motor5_can_set_current, 0, 0);          //m2006发送控制参数
        
        if(arm_control.arm_motor6_motor.arm_motor_mode==ARM_MOTOR_RAW)//当机械臂为无力模式的时候，全部电机模式为ARM_MOTOR_RAW模式
        {
            DM_mit_ctrl(FDCAN_DM_J1_ID, joint1_can_set_current, 0, 0, 0, 0);//kp,kd设置为0没有力矩输出
            M8010_CMD_SEND(USART_M8010_J2_ID,0,0,0, joint2_usart_set_current,JOINT2_M8010_KP,JOINT2_M8010_KD);//树宇M8010模式设置为模式0
            DM_mit_ctrl(FDCAN_DM_J4_ID, joint4_can_set_current, 0, 0, 0, 0);    
            M8010_CMD_SEND(USART_M8010_J3_ID,0,0,0, joint3_usart_set_current,JOINT3_M8010_KP,JOINT3_M8010_KD);   
        }else if (arm_control.arm_motor6_motor.arm_motor_mode==ARM_MOTOR_ENCONDE)
        {
            DM_mit_ctrl(FDCAN_DM_J1_ID, joint1_can_set_current, 0, JOINT1_DM4310_KP, JOINT1_DM4310_KD, 0);
            M8010_CMD_SEND(USART_M8010_J2_ID,1,0,0, joint2_usart_set_current,JOINT2_M8010_KP,JOINT2_M8010_KD);//树宇M8010模式设置为模式1，发送控制参数
            DM_mit_ctrl(FDCAN_DM_J4_ID, joint4_can_set_current, 0, JOINT4_DM4310_KP, JOINT4_DM4310_KD, 0);    //达妙4310发送控制参数
            M8010_CMD_SEND(USART_M8010_J3_ID,1,0,0, joint3_usart_set_current,JOINT3_M8010_KP,JOINT3_M8010_KD);
        }
    }
}

/**
 * @brief 初始化机械臂关节偏移量与角度范围
 * 初始位姿的电机偏移量根据实际情况修改。
 */
void arm_offset_init(arm_control_t *init)
{
    arm_control.arm_joint1_motor.offset_rad = -1.807622f;
    arm_control.arm_joint1_motor.max_relative_angle = PI;
    arm_control.arm_joint1_motor.min_relative_angle = -PI;

    arm_control.arm_joint2_motor.offset_rad = init->arm_joint2_motor.arm_motor_measure->Pos;//上电记录输出轴位置作为电机偏移
    arm_control.arm_joint2_motor.max_relative_angle = PI;
    arm_control.arm_joint2_motor.min_relative_angle = 0;

    arm_control.arm_joint3_motor.offset_rad = init->arm_joint3_motor.arm_motor_measure->Pos;
    arm_control.arm_joint3_motor.max_relative_angle = 2*PI/3;
    arm_control.arm_joint3_motor.min_relative_angle = -2*PI/3;

    arm_control.arm_joint4_motor.offset_rad = -1.277752f;
    arm_control.arm_joint4_motor.max_relative_angle = PI;
    arm_control.arm_joint4_motor.min_relative_angle = -PI;

    arm_control.arm_motor5_motor.offset_ecd = init->arm_motor5_motor.arm_motor_measure->ecd;
    arm_control.arm_motor5_motor.max_relative_angle = PI;
    arm_control.arm_motor5_motor.min_relative_angle = -PI;

    arm_control.arm_motor6_motor.offset_ecd = init->arm_motor6_motor.arm_motor_measure->ecd;
    arm_control.arm_motor6_motor.max_relative_angle = PI;
    arm_control.arm_motor6_motor.min_relative_angle = -PI;
}

/**
  * @brief          初始化"arm_control"变量，包括pid初始化， 遥控器指针初始化，机械臂电机指针初始化
  * @param[out]     init:"arm_control"变量指针.
  * @retval         none
  */
static void arm_init(arm_control_t *init)
{
    static const fp32 motor5_speed_pid[3] = {MOTOR5_SPEED_PID_KP, MOTOR5_SPEED_PID_KI, MOTOR5_SPEED_PID_KD};
    static const fp32 motor6_speed_pid[3] = {MOTOR6_SPEED_PID_KP, MOTOR6_SPEED_PID_KI, MOTOR6_SPEED_PID_KD};
    //电机数据指针获取
    init->arm_joint1_motor.arm_motor_measure = get_DM_Jiont1_Measure_Point();
    init->arm_joint4_motor.arm_motor_measure = get_DM_Joint4_Measure_Point();
    init->arm_motor5_motor.arm_motor_measure = get_RM_motor5_Motor_Measure_Point();
    init->arm_motor6_motor.arm_motor_measure = get_RM_motor6_Motor_Measure_Point();
    init->arm_joint2_motor.arm_motor_measure = get_UN_Joint2_Motor_Measure_Point();
    init->arm_joint3_motor.arm_motor_measure = get_UN_Joint3_Motor_Measure_Point();    

    //遥控器数据指针获取
    //  init->arm_rc_ctrl = get_remote_control_point();
    
    //防止宇树电机初始化，反馈错误的位置
    while (init->arm_joint2_motor.arm_motor_measure->Pos==0||init->arm_joint3_motor.arm_motor_measure->Pos==0)    
    {   static int time_out=0;
        M8010_CMD_SEND(USART_M8010_J2_ID,0,0,0,0,0,0);
        HAL_Delay(10);
        M8010_CMD_SEND(USART_M8010_J3_ID,0,0,0,0,0,0);
        time_out++;
        if(time_out>500) break;
    }
    //达妙电机使能
    DM_enable_motor_mode(FDCAN_DM_J1_ID,MIT_MODE);
    DM_enable_motor_mode(FDCAN_DM_J4_ID,MIT_MODE);

    //初始化电机模式wwww
    init->arm_joint1_motor.arm_motor_mode = init->arm_joint1_motor.last_arm_motor_mode = ARM_MOTOR_RAW;
    init->arm_joint2_motor.arm_motor_mode = init->arm_joint2_motor.last_arm_motor_mode = ARM_MOTOR_RAW;
    init->arm_joint3_motor.arm_motor_mode = init->arm_joint3_motor.last_arm_motor_mode = ARM_MOTOR_RAW;
    init->arm_joint4_motor.arm_motor_mode = init->arm_joint4_motor.last_arm_motor_mode = ARM_MOTOR_RAW;
    init->arm_motor5_motor.arm_motor_mode = init->arm_motor5_motor.last_arm_motor_mode = ARM_MOTOR_RAW;
    init->arm_motor6_motor.arm_motor_mode = init->arm_motor6_motor.last_arm_motor_mode = ARM_MOTOR_RAW;

    //初始化motor5电机pid
    arm_PID_init(&init->arm_motor5_motor.arm_motor_relative_angle_pid, motor5_ENCODE_RELATIVE_PID_MAX_OUT, motor5_ENCODE_RELATIVE_PID_MAX_IOUT, motor5_ENCODE_RELATIVE_PID_KP, motor5_ENCODE_RELATIVE_PID_KI, motor5_ENCODE_RELATIVE_PID_KD);
    PID_init(&init->arm_motor5_motor.arm_motor_gyro_pid, PID_POSITION, motor5_speed_pid, MOTOR5_SPEED_PID_MAX_OUT, MOTOR5_SPEED_PID_MAX_IOUT);
    //初始化motor6电机pid
    arm_PID_init(&init->arm_motor6_motor.arm_motor_relative_angle_pid, motor6_ENCODE_RELATIVE_PID_MAX_OUT, motor6_ENCODE_RELATIVE_PID_MAX_IOUT, motor6_ENCODE_RELATIVE_PID_KP, motor6_ENCODE_RELATIVE_PID_KI, motor6_ENCODE_RELATIVE_PID_KD);
    PID_init(&init->arm_motor6_motor.arm_motor_gyro_pid, PID_POSITION, motor6_speed_pid, MOTOR6_SPEED_PID_MAX_OUT, MOTOR6_SPEED_PID_MAX_IOUT);

    //清除所有PID
    arm_total_pid_clear(init);

    arm_feedback_update(init);
    init->arm_motor5_motor.ecd_count = 0;
    init->arm_motor6_motor.ecd_count = 0;
    //将电机初始的角度与角速度初始化为目标角度与速度
    init->arm_joint1_motor.relative_angle_set = init->arm_joint1_motor.relative_angle;
    init->arm_joint2_motor.relative_angle_set = init->arm_joint2_motor.relative_angle;
    init->arm_joint3_motor.relative_angle_set = init->arm_joint3_motor.relative_angle;
    init->arm_joint4_motor.relative_angle_set = init->arm_joint4_motor.relative_angle;
    
    init->arm_motor5_motor.relative_angle_set = init->arm_motor5_motor.relative_angle;
    init->arm_motor5_motor.motor_gyro_set = init->arm_motor5_motor.motor_gyro;
    init->arm_motor6_motor.relative_angle_set = init->arm_motor6_motor.relative_angle;
    init->arm_motor6_motor.motor_gyro_set = init->arm_motor6_motor.motor_gyro;

}

/**
  * @brief          设置机械臂控制模式，主要在'arm_behaviour_mode_set'函数中改变
  * @param[out]     arm_set_mode:"arm_control"变量指针.
  * @retval         none
  */
static void arm_set_mode(arm_control_t *set_mode)
{
    if (set_mode == NULL)
    {
        return;
    }
    arm_behaviour_mode_set(set_mode);
}

/**
  * @brief          机械臂测量数据更新，包括电机速度，欧拉角度
  * @param[out]     arm_feedback_update:"arm_control"变量指针.
  * @retval         none
  */
static void arm_feedback_update(arm_control_t *feedback_update)
{

    if (feedback_update == NULL)
    {
        return;
    }
    /*------------------m2006电机速度低通滤波参数------------------*/
    static fp32 motor5_speed_fliter_1 = 0.0f;
    static fp32 motor5_speed_fliter_2 = 0.0f;
    static fp32 motor5_speed_fliter_3 = 0.0f;
    static fp32 motor6_speed_fliter_1 = 0.0f;
    static fp32 motor6_speed_fliter_2 = 0.0f;
    static fp32 motor6_speed_fliter_3 = 0.0f;
    static const fp32 fliter_num[3] = {1.725709860247969f, -0.75594777109163436f, 0.030237910843665373f};    //M2006电机速度二阶低通滤波参数,根据实际情况进行修改
    /*------------------m2006电机速度低通滤波参数------------------*/
    //机械臂数据更新
#if JOINT1_TURN
    feedback_update->arm_joint1_motor.relative_angle =feedback_update->arm_joint1_motor.arm_motor_measure->pos-feedback_update->arm_joint1_motor.offset_rad;
    //printf("offset_rad=%f\n",feedback_update->arm_joint1_motor.arm_motor_measure->pos);
#else
    feedback_update->arm_joint1_motor.relative_angle = -(feedback_update->arm_joint1_motor.arm_motor_measure->pos-feedback_update->arm_joint1_motor.offset_rad);
#endif
    feedback_update->arm_joint1_motor.motor_gyro = feedback_update->arm_joint1_motor.arm_motor_measure->vel;
    if(feedback_update->arm_joint1_motor.arm_motor_measure->err==0)//防止机器人掉电达妙电机失能，重新使能
    {
        DM_enable_motor_mode(FDCAN_DM_J1_ID,MIT_MODE);
        //printf("FDCAN_DM_J1_enable\n");
    }
#if JOINT4_TURN
    feedback_update->arm_joint4_motor.relative_angle =feedback_update->arm_joint4_motor.arm_motor_measure->pos-feedback_update->arm_joint4_motor.offset_rad;
    // printf("offset_rad=%f\n",feedback_update->arm_joint4_motor.arm_motor_measure->pos);
#else
    feedback_update->arm_joint4_motor.relative_angle = -(feedback_update->arm_joint4_motor.arm_motor_measure->pos-feedback_update->arm_joint4_motor.offset_rad);
#endif
    feedback_update->arm_joint4_motor.motor_gyro = feedback_update->arm_joint4_motor.arm_motor_measure->vel;
    if(feedback_update->arm_joint4_motor.arm_motor_measure->err==0)//防止机器人掉电达妙电机失能，重新使能
    {
        DM_enable_motor_mode(FDCAN_DM_J4_ID,MIT_MODE);
        //printf("FDCAN_DM_J4_enable\n");
    }


    //RM电机（M2006）反馈更新，计算motor5输出轴角度以及反馈角速度
#if MOTOR5_TURN
    feedback_update->arm_motor5_motor.relative_angle =  (feedback_update->arm_motor5_motor.arm_motor_measure->ecd_count * ECD_RANGE + 
        feedback_update->arm_motor5_motor.arm_motor_measure->ecd - feedback_update->arm_motor5_motor.offset_ecd) * M2006_MOTOR_ECD_TO_ANGLE;
#else
    feedback_update->arm_motor5_motor.relative_angle =  -(feedback_update->arm_motor5_motor.arm_motor_measure->ecd_count * ECD_RANGE + 
        feedback_update->arm_motor5_motor.arm_motor_measure->ecd - feedback_update->arm_motor5_motor.offset_ecd) * M2006_MOTOR_ECD_TO_ANGLE;
#endif
    //二阶低通滤波
    motor5_speed_fliter_1 = motor5_speed_fliter_2;
    motor5_speed_fliter_2 = motor5_speed_fliter_3;
    motor5_speed_fliter_3 = motor5_speed_fliter_2 * fliter_num[0] + motor5_speed_fliter_1 * fliter_num[1] + (feedback_update->arm_motor5_motor.arm_motor_measure->speed_rpm*M2006_MOTOR_RPM_TO_SPEED) * fliter_num[2];
    feedback_update->arm_motor5_motor.motor_gyro = motor5_speed_fliter_3;

    //计算motor6输出轴角度以及反馈角速度
#if MOTOR6_TURN
    feedback_update->arm_motor6_motor.relative_angle = (feedback_update->arm_motor6_motor.arm_motor_measure->ecd_count * ECD_RANGE + 
                feedback_update->arm_motor6_motor.arm_motor_measure->ecd - feedback_update->arm_motor6_motor.offset_ecd) * M2006_MOTOR_ECD_TO_ANGLE;
#else
    feedback_update->arm_motor6_motor.relative_angle = -(feedback_update->arm_motor6_motor.arm_motor_measure->ecd_count * ECD_RANGE + 
                feedback_update->arm_motor6_motor.arm_motor_measure->ecd - feedback_update->arm_motor6_motor.offset_ecd) * M2006_MOTOR_ECD_TO_ANGLE;
#endif
    printf("ecd_count=%d\n",feedback_update->arm_motor6_motor.arm_motor_measure->ecd_count);
    //二阶低通滤波
    motor6_speed_fliter_1 = motor6_speed_fliter_2;
    motor6_speed_fliter_2 = motor6_speed_fliter_3;
    motor6_speed_fliter_3 = motor6_speed_fliter_2 * fliter_num[0] + motor6_speed_fliter_1 * fliter_num[1] + (feedback_update->arm_motor6_motor.arm_motor_measure->speed_rpm*M2006_MOTOR_RPM_TO_SPEED) * fliter_num[2];
    feedback_update->arm_motor6_motor.motor_gyro = motor6_speed_fliter_3;
/*----------------------------计算关节5，6的反馈角度---------------------------*/
    //关节5的角度变化存到电机5的结构体，关节6存到电机6的结构体中
    feedback_update->arm_motor5_motor.joint_angel=(feedback_update->arm_motor6_motor.relative_angle
                                    -feedback_update->arm_motor5_motor.relative_angle)/2/JOINT5_TO_MOTOR5_6_SCALE;
    feedback_update->arm_motor6_motor.joint_angel=(feedback_update->arm_motor6_motor.relative_angle
                                    +feedback_update->arm_motor5_motor.relative_angle)/2/JOINT6_TO_MOTOR5_6_SCALE;
/*----------------------------计算关节5，6的反馈角度---------------------------*/
//树宇电机（M8010）反馈更新
#if JOINT2_TURN
    feedback_update->arm_joint2_motor.relative_angle =feedback_update->arm_joint2_motor.arm_motor_measure->Pos - feedback_update->arm_joint2_motor.offset_rad;
    //printf("relative_angle2=%f,Pos=%f offset_rad=%f\n",feedback_update->arm_joint2_motor.relative_angle,feedback_update->arm_joint2_motor.arm_motor_measure->Pos,feedback_update->arm_joint2_motor.offset_rad );
#else
    feedback_update->arm_joint2_motor.relative_angle = -feedback_update->arm_joint2_motor.arm_motor_measure->Pos - feedback_update->arm_joint2_motor.offset_rad;
#endif
    feedback_update->arm_joint2_motor.motor_gyro = feedback_update->arm_joint2_motor.arm_motor_measure->W;    

#if JOINT3_TURN
    feedback_update->arm_joint3_motor.relative_angle =feedback_update->arm_joint3_motor.arm_motor_measure->Pos - feedback_update->arm_joint3_motor.offset_rad;
   
#else
    feedback_update->arm_joint3_motor.relative_angle = -(feedback_update->arm_joint3_motor.arm_motor_measure->Pos-feedback_update->arm_joint3_motor.offset_rad);
     //printf("relative_angle3=%f,Pos=%f offset_rad=%f\n",feedback_update->arm_joint3_motor.relative_angle,feedback_update->arm_joint3_motor.arm_motor_measure->Pos,feedback_update->arm_joint3_motor.offset_rad );
#endif
    feedback_update->arm_joint3_motor.motor_gyro = feedback_update->arm_joint3_motor.arm_motor_measure->W;//w树宇电机反馈速度
}

/**
  * @brief          机械臂模式改变，有些参数需要改变，例如控制yaw角度设定值应该变成当前yaw角度
  * @param[out]     arm_mode_change:"arm_control"变量指针.
  * @retval         none
  */
static void arm_mode_change_control_transit(arm_control_t *arm_mode_change)
{
    if (arm_mode_change == NULL)
    {
        return;
    }
    //joint1电机状态机切换保存数据
    if (arm_mode_change->arm_joint1_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_joint1_motor.raw_cmd_current = arm_mode_change->arm_joint1_motor.current_set = arm_mode_change->arm_joint1_motor.given_current;
    }
    else if (arm_mode_change->arm_joint1_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_joint1_motor.relative_angle_set = arm_mode_change->arm_joint1_motor.relative_angle;
    }
    arm_mode_change->arm_joint1_motor.last_arm_motor_mode = arm_mode_change->arm_joint1_motor.arm_motor_mode;

    //joint2电机状态机切换保存数据
    if (arm_mode_change->arm_joint2_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_joint2_motor.raw_cmd_current = arm_mode_change->arm_joint2_motor.current_set = arm_mode_change->arm_joint2_motor.given_current;
    }
    else if (arm_mode_change->arm_joint2_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_joint2_motor.relative_angle_set = arm_mode_change->arm_joint2_motor.relative_angle;
    }
    arm_mode_change->arm_joint2_motor.last_arm_motor_mode = arm_mode_change->arm_joint2_motor.arm_motor_mode;

        //joint3电机状态机切换保存数据
    if (arm_mode_change->arm_joint3_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_joint3_motor.raw_cmd_current = arm_mode_change->arm_joint3_motor.current_set = arm_mode_change->arm_joint3_motor.given_current;
    }
    else if (arm_mode_change->arm_joint3_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_joint3_motor.relative_angle_set = arm_mode_change->arm_joint3_motor.relative_angle;
    }
    arm_mode_change->arm_joint3_motor.last_arm_motor_mode = arm_mode_change->arm_joint3_motor.arm_motor_mode;

        //joint4电机状态机切换保存数据
    if (arm_mode_change->arm_joint4_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_joint4_motor.raw_cmd_current = arm_mode_change->arm_joint4_motor.current_set = arm_mode_change->arm_joint4_motor.given_current;
    }
    else if (arm_mode_change->arm_joint4_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_joint4_motor.relative_angle_set = arm_mode_change->arm_joint4_motor.relative_angle;
    }
    arm_mode_change->arm_joint4_motor.last_arm_motor_mode = arm_mode_change->arm_joint4_motor.arm_motor_mode;

        //motor5电机状态机切换保存数据
    if (arm_mode_change->arm_motor5_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_motor5_motor.raw_cmd_current = arm_mode_change->arm_motor5_motor.current_set = arm_mode_change->arm_motor5_motor.given_current;
    }
    else if (arm_mode_change->arm_motor5_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_motor5_motor.relative_angle_set = arm_mode_change->arm_motor5_motor.relative_angle;
    }
    arm_mode_change->arm_motor5_motor.last_arm_motor_mode = arm_mode_change->arm_motor5_motor.arm_motor_mode;

        //motor6电机状态机切换保存数据
    if (arm_mode_change->arm_motor6_motor.last_arm_motor_mode != ARM_MOTOR_RAW && arm_mode_change->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        arm_mode_change->arm_motor6_motor.raw_cmd_current = arm_mode_change->arm_motor6_motor.current_set = arm_mode_change->arm_motor6_motor.given_current;
    }
    else if (arm_mode_change->arm_motor6_motor.last_arm_motor_mode != ARM_MOTOR_ENCONDE && arm_mode_change->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        arm_mode_change->arm_motor6_motor.relative_angle_set = arm_mode_change->arm_motor6_motor.relative_angle;
    }
    arm_mode_change->arm_motor6_motor.last_arm_motor_mode = arm_mode_change->arm_motor6_motor.arm_motor_mode;

}

/**
  * @brief          设置机械臂控制设定值，控制值是通过arm_behaviour_control_set函数设置的
  *                 该函数相当于是获取控制值
  * @param[out]     set_control:"arm_control"变量指针.
  * @retval         none
  */
static void arm_set_control(arm_control_t *set_control)
{
    if (set_control == NULL)
    {
        return;
    }
    //wwww
    fp32 joint1_angle = 0;
    fp32 joint2_angle = 0;//-PI;
    fp32 joint3_angle = 0;
    fp32 joint4_angle = 0;
    fp32 motor5_angle = 0;
    fp32 motor6_angle = 0; 
    //PID速度环调试参数设置
    fp32 jointx_speed = 10;//20
     //set_control->arm_motor5_motor.motor_gyro_set=jointx_speed;
   //获取机械臂控制角度
   //arm_behaviour_control_set(&joint1_angle, &joint2_angle, &joint3_angle, &joint4_angle, &motor5_angle, &motor6_angle, set_control);
    //joint1电机模式控制
    if (set_control->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_joint1_motor.raw_cmd_current = joint1_angle;
    }
    else if (set_control->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        DM4310_joint_relative_angle_limit(&set_control->arm_joint1_motor, joint1_angle);
    }
    //joint2电机模式控制
    if (set_control->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_joint2_motor.raw_cmd_current = joint2_angle;
    }
    else if (set_control->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        M8010_joint_relative_angle_limit(&set_control->arm_joint2_motor, joint2_angle);
    }
    //joint3电机模式控制
    if (set_control->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_joint3_motor.raw_cmd_current = joint3_angle;
    }
    else if (set_control->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        M8010_joint_relative_angle_limit(&set_control->arm_joint3_motor, joint3_angle);
    }
    //joint4电机模式控制
    if (set_control->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_joint4_motor.raw_cmd_current = joint4_angle;
    }
    else if (set_control->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        DM4310_joint_relative_angle_limit(&set_control->arm_joint4_motor, joint4_angle);
    }
    //motor5电机模式控制
    if (set_control->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_motor5_motor.raw_cmd_current = motor5_angle;
    }
    else if (set_control->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        RM2006_joint_relative_angle_limit(&set_control->arm_motor5_motor, motor5_angle);
    }
    //motor6电机模式控制
    if (set_control->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        set_control->arm_motor6_motor.raw_cmd_current = motor6_angle;
    }
    else if (set_control->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
        RM2006_joint_relative_angle_limit(&set_control->arm_motor6_motor, motor6_angle);
    }

}

/**
  * @brief          机械臂控制模式:ARM_MOTOR_ENCONDE，使用编码相对角进行控制
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
static void RM2006_joint_relative_angle_limit(arm_rm_motor_t *arm_motor,fp32 joint_angle)
{
    if (arm_motor == NULL)
    {
        return;
    }
    arm_motor->relative_angle_set = joint_angle;
    //是否超过最大 最小值
    if (arm_motor->relative_angle_set > arm_motor->max_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->max_relative_angle;
    }
    else if (arm_motor->relative_angle_set < arm_motor->min_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->min_relative_angle;
    }
}
void DM4310_joint_relative_angle_limit(DM4310_Motor_t *arm_motor,fp32 joint_angle)
{
    if (arm_motor == NULL)
    {
        return;
    }
    arm_motor->relative_angle_set = joint_angle;
    //是否超过最大 最小值
    if (arm_motor->relative_angle_set > arm_motor->max_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->max_relative_angle;
    }
    else if (arm_motor->relative_angle_set < arm_motor->min_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->min_relative_angle;
    }
}
void M8010_joint_relative_angle_limit(M8010_Motor_t *arm_motor,fp32 joint_angle)
{
    if (arm_motor == NULL)
    {
        return;
    }

    arm_motor->relative_angle_set = joint_angle;
    //是否超过最大 最小值
    if (arm_motor->relative_angle_set > arm_motor->max_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->max_relative_angle;
    }
    else if (arm_motor->relative_angle_set < arm_motor->min_relative_angle)
    {
        arm_motor->relative_angle_set = arm_motor->min_relative_angle;
    }
}
/**
  * @brief          控制循环，根据控制设定值，计算电机电流值，进行控制
  * @param[out]     arm_control_loop:"arm_control"变量指针.
  * @retval         none
  */
static void arm_control_loop(arm_control_t *control_loop)
{
    if (control_loop == NULL)
    {
        return;
    }
    if (control_loop->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        DM4310_joint_raw_angle_control(&control_loop->arm_joint1_motor);
    }
    else if (control_loop->arm_joint1_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        DM4310_joint_relative_angle_control(&control_loop->arm_joint1_motor);
    }

    if (control_loop->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        DM4310_joint_raw_angle_control(&control_loop->arm_joint4_motor);
    }
    else if (control_loop->arm_joint4_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        DM4310_joint_relative_angle_control(&control_loop->arm_joint4_motor);
    }

    if (control_loop->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        M8010_joint_raw_angle_control(&control_loop->arm_joint2_motor);
    }
    else if (control_loop->arm_joint2_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        M8010_joint_relative_angle_control(&control_loop->arm_joint2_motor);
    }

    if (control_loop->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        M8010_joint_raw_angle_control(&control_loop->arm_joint3_motor);
    }
    else if (control_loop->arm_joint3_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        M8010_joint_relative_angle_control(&control_loop->arm_joint3_motor);
    }

    if (control_loop->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        RM2006_joint_raw_angle_control(&control_loop->arm_motor5_motor);
    }
    else if (control_loop->arm_motor5_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        RM2006_joint_relative_angle_control(&control_loop->arm_motor5_motor);
    }

    if (control_loop->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_RAW)
    {
        RM2006_joint_raw_angle_control(&control_loop->arm_motor6_motor);
    }
    else if (control_loop->arm_motor6_motor.arm_motor_mode == ARM_MOTOR_ENCONDE)
    {
        RM2006_joint_relative_angle_control(&control_loop->arm_motor6_motor);
    }
}

/**
  * @brief          机械臂控制模式:ARM_MOTOR_ENCONDE，使用编码相对角进行控制
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
 //waw
static void RM2006_joint_relative_angle_control(arm_rm_motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    //角度环，速度环串级pid调试
    arm_motor->motor_gyro_set = arm_PID_calc(&arm_motor->arm_motor_relative_angle_pid, arm_motor->relative_angle, arm_motor->relative_angle_set, arm_motor->motor_gyro);
    arm_motor->current_set = PID_calc(&arm_motor->arm_motor_gyro_pid, arm_motor->motor_gyro, arm_motor->motor_gyro_set);
    //控制值赋值
    if(arm_motor==&(arm_control.arm_motor5_motor))
    {
#if MOTOR5_TURN 
    arm_motor->given_current = (int16_t)(arm_motor->current_set);
#else
    arm_motor->given_current = (int16_t)(-arm_motor->current_set);
#endif
    }
    if(arm_motor==&(arm_control.arm_motor6_motor))
    {
#if MOTOR6_TURN
    arm_motor->given_current = (int16_t)(arm_motor->current_set);
#else
    arm_motor->given_current = (int16_t)(-arm_motor->current_set);
#endif
    }   
}

//达妙电机采用电机内部的MIT模式的位置控制，直接发送控制位置
static void DM4310_joint_relative_angle_control(DM4310_Motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    if(arm_motor==&(arm_control.arm_joint1_motor))
    {
#if JOINT1_TURN
    arm_motor->current_set = arm_motor->relative_angle_set ;
#else
    arm_motor->current_set = -arm_motor->relative_angle_set ;
#endif
    }
    if(arm_motor==&(arm_control.arm_joint4_motor))
    {
#if JOINT4_TURN
    arm_motor->current_set = arm_motor->relative_angle_set ;
#else
    arm_motor->current_set = -arm_motor->relative_angle_set ;
#endif
    }
    arm_motor->given_current = arm_motor->current_set + arm_motor->offset_rad;
}

//宇树电机采用电机内部的位置模式的控制方式，直接发送控制位置
static void M8010_joint_relative_angle_control(M8010_Motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    if(arm_motor==&(arm_control.arm_joint2_motor))
    {
#if JOINT2_TURN
    arm_motor->current_set = arm_motor->relative_angle_set * M8010_MOTOR_SCALE ;//电机转子与输出轴的减速比6.33
#else
    arm_motor->current_set = -arm_motor->relative_angle_set * M8010_MOTOR_SCALE ;
#endif
    arm_motor->given_current = arm_motor->current_set * M8010_MOTOR2JOINT_SCALE + arm_motor->offset_rad;//电机输出轴与关节减速比3.0
    }
    if(arm_motor==&(arm_control.arm_joint3_motor))
    {
#if JOINT3_TURN
    arm_motor->current_set = arm_motor->relative_angle_set * M8010_MOTOR_SCALE ;//电机转子与输出轴的减速比6.33
#else
    arm_motor->current_set = -arm_motor->relative_angle_set * M8010_MOTOR_SCALE ;
#endif
    arm_motor->given_current = arm_motor->current_set + arm_motor->offset_rad;
    }    
}

/**
  * @brief          机械臂控制模式:ARM_MOTOR_RAW，电流值直接发送到数据总线.
  * @param[out]     arm_motor:jointx
  * @retval         none
  */
static void RM2006_joint_raw_angle_control(arm_rm_motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    arm_motor->current_set = arm_motor->raw_cmd_current;
    arm_motor->given_current = (int16_t)(arm_motor->current_set);
}
static void DM4310_joint_raw_angle_control(DM4310_Motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    arm_motor->current_set = arm_motor->raw_cmd_current;
    arm_motor->given_current = arm_motor->current_set;
}
static void M8010_joint_raw_angle_control(M8010_Motor_t *arm_motor)
{
    if (arm_motor == NULL)
    {
        return;
    }
    arm_motor->current_set = arm_motor->raw_cmd_current;
    arm_motor->given_current = (int16_t)(arm_motor->current_set);
}

/**
  * @brief          初始化"arm_control"变量，包括pid初始化， 遥控器指针初始化，机械臂电机指针初始化，陀螺仪角度指针初始化
  * @param[out]     arm_init:"arm_control"变量指针.
  * @retval         none
  */
static void arm_PID_init(arm_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
{
    if (pid == NULL)
    {
        return;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->err = 0.0f;
    pid->get = 0.0f;

    pid->max_iout = max_iout;
    pid->max_out = maxout;
}

static fp32 arm_PID_calc(arm_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
{
    fp32 err;
    if (pid == NULL)
    {
        return 0.0f;
    }
    pid->get = get;
    pid->set = set;
    err = set - get;
   
    pid->err = err; 
    pid->Pout = pid->kp * pid->err;
    pid->Iout += pid->ki * pid->err;
    pid->Dout = pid->kd * error_delta;
    abs_limit(&pid->Iout, pid->max_iout);
    pid->out = pid->Pout + pid->Iout + pid->Dout;
    abs_limit(&pid->out, pid->max_out);
    return pid->out;
}

/**
  * @brief          机械臂PID清除，清除pid的out,iout
  * @param[out]     arm_pid_clear:"arm_control"变量指针.
  * @retval         none
  */
static void arm_PID_clear(arm_PID_t *arm_pid_clear)
{
    if (arm_pid_clear == NULL)
    {
        return;
    }
    arm_pid_clear->err = arm_pid_clear->set = arm_pid_clear->get = 0.0f;
    arm_pid_clear->out = arm_pid_clear->Pout = arm_pid_clear->Iout = arm_pid_clear->Dout = 0.0f;
}
